#include <cv.h>
#include <highgui.h>
#include "../../IA/src/OutilsEntree.h"
#include "acquisition-outils.h"
#include <time.h>

// ---------------------------------------------------------------------------
// Fonctions de traitement des images et d'isolation d'une couleur
void  update_mhi( IplImage* img, IplImage* dst, int diff_threshold, 
						int tailleMin, int tailleMax, positionRectangle rectangles[], int* nbObjets, int nbmaxObjets)
// update_mhi : permet de détecter les objets du terrain.
//					les fonctions utilisées servent à l'origine à détecter les mouvements
//					mais on ne compare plus l'image avec la précédente mais avec une image complètement noire
//					puis on ne garde que les objets correspondants aux limites de taille
// parametres:
// img :			image à analyser
// dst :			image résultante, peu intéressante
// diff_threshold :	
// tailleMin :		taille minimale des objets à détecter
// tailleMax :		taille maximake des objets à détecter
// rectangles :		tableau des objets détectés. Doit etre alloué !!!!!
// nbObjets :		nombre d'objets détectés dans l'image
// nbmaxObjets :	nombre max d'objets à détecter, taille maximale de rectangles[]
{
    double timestamp = (double)clock()/CLOCKS_PER_SEC; // get current time in seconds
    CvSize size = cvSize(img->width,img->height); // get current frame size
    int i;
    IplImage* silh;
    CvSeq* seq;
    CvRect comp_rect;
    double count;
    CvPoint center;
    double magnitude;          
    CvScalar color;
    CvMemStorage* storage = cvCreateMemStorage(0);
	// various tracking parameters (in seconds)
	const double MHI_DURATION = 1;
	const double MAX_TIME_DELTA = 0.5;
	const double MIN_TIME_DELTA = 0.05;
	
	IplImage *buf1 = 0;
	IplImage *buf2 = 0;
	
	// temporary images
	IplImage *mhi = 0; // MHI
	IplImage *orient = 0; // orientation
	IplImage *mask = 0; // valid orientation mask
	IplImage *segmask = 0; // motion segmentation map
    
    buf1 	= cvCreateImage( size, IPL_DEPTH_8U, 1 );
    buf2 	= cvCreateImage( size, IPL_DEPTH_8U, 1 );
    mhi 	= cvCreateImage( size, IPL_DEPTH_32F, 1 );
    orient	= cvCreateImage( size, IPL_DEPTH_32F, 1 );
    segmask	= cvCreateImage( size, IPL_DEPTH_32F, 1 );
    mask	= cvCreateImage( size, IPL_DEPTH_8U, 1 );
    cvZero( mhi ); // clear MHI at the beginning
    cvZero( buf1 );
    cvZero( buf2 );
    
    cvCvtColor( img, buf1, CV_BGR2GRAY ); // convert frame to grayscale

    silh = buf2;
    cvAbsDiff( buf1, buf2, silh ); // get difference between frames
    
    cvThreshold( silh, silh, diff_threshold, 1, CV_THRESH_BINARY ); // and threshold it
    cvUpdateMotionHistory( silh, mhi, timestamp, MHI_DURATION ); // update MHI

    // convert MHI to blue 8u image
    cvCvtScale( mhi, mask, 255./MHI_DURATION,
                (MHI_DURATION - timestamp)*255./MHI_DURATION );
    cvZero( dst );
    cvCvtPlaneToPix( mask, 0, 0, 0, dst );
    
    // calculate motion gradient orientation and valid orientation mask
    cvCalcMotionGradient( mhi, mask, orient, MAX_TIME_DELTA, MIN_TIME_DELTA, 3 );
    
    // segment motion: get sequence of motion components
    // segmask is marked motion components map. It is not used further
    seq = cvSegmentMotion( mhi, segmask, storage, timestamp, MAX_TIME_DELTA );

    // iterate through the motion components,
    // One more iteration (i == -1) corresponds to the whole image (global motion)
    (*nbObjets) = 0;
    for( i = 0; i < seq->total; i++ )
    {
        comp_rect = ((CvConnectedComp*)cvGetSeqElem( seq, i ))->rect;
        magnitude = (comp_rect.width + comp_rect.height) /2;
        if( comp_rect.width <= tailleMin || comp_rect.width >tailleMax || 
        		comp_rect.height <= tailleMin || comp_rect.height >tailleMax) // reject very small components
            continue;
        color = CV_RGB(255,0,0);

        // select component ROI
        cvSetImageROI( silh, comp_rect );
        cvSetImageROI( mhi, comp_rect );
        cvSetImageROI( mask, comp_rect );

        count = cvNorm( silh, 0, CV_L1, 0 ); // calculate number of points within silhouette ROI

        cvResetImageROI( mhi );
        cvResetImageROI( mask );
        cvResetImageROI( silh );

        // check for the case of little motion
        if( count < comp_rect.width*comp_rect.height * 0.05 )
            continue;

        // draw a clock with arrow indicating the direction
        center = cvPoint( (comp_rect.x + comp_rect.width/2),
                          (comp_rect.y + comp_rect.height/2) );
        
        
        rectangles[*nbObjets].center.x = center.x;
        rectangles[*nbObjets].center.y = center.y;
        rectangles[*nbObjets].width  = comp_rect.width ;
        rectangles[*nbObjets].height = comp_rect.height;
        (*nbObjets)++;
        if ((*nbObjets)>=nbmaxObjets)
        {
        	break;
        }
    }
    cvReleaseImage( &mhi );
    cvReleaseImage( &orient );
    cvReleaseImage( &segmask );
    cvReleaseImage( &mask );
    cvReleaseImage( &buf1 );
    cvReleaseImage( &buf2 );
    cvReleaseMemStorage(&storage);
    // avant : cvClearmemStorage(storage); /: pas de déallocation : fuite de mémoire
}
int traiterrouge(IplImage* hsvFrame)
// traiterrouge transforme l'image entrante en rouge
//					fonctionnement : les pixels non rouge ou trop sombre ou trop gris sont transformés en noir
// <hsvFrame> : image qui doit etre allouée, en codage HSV
{
	//Pour chaque pixel…
	int x, y;
	int val_H, val_S, val_V;
	for(y = 0; y < (cvGetSize(hsvFrame).height); y++) 
	{
		for(x = 0; x < (cvGetSize(hsvFrame).width); x++) 
		{
			val_H = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3  ];
			val_S = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+1];
			val_V = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+2];
			
			if(!(	((val_H >=-85 && val_H <=-75) || (val_H >=0 && val_H <=10) ) &&
					( val_S<0 ) &&
					( val_V<=0) ) )
			// rouge orangé  :   0 à  10
			// rouge violacé : -85 à -75 (-90 à -75 pour plus grande plage)
			// peu grisé : saturation < 0 (pas le blanc)
			// image suffisemment claire : valeur <= 0
			{
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+1] = 0;
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+2] = 0;
			}
		}
	} //fin des boucles
	
    return 0;
}

int traitervert(IplImage* hsvFrame)
// traitervert transforme l'image entrante en vert
//					fonctionnement : les pixels non vert ou trop sombre ou trop gris sont transformés en noir
// <hsvFrame> : image qui doit etre allouée, en codage HSV
{
	//Pour chaque pixel…
	int x, y;
	int val_H, val_S, val_V;
	for(y = 0; y < (cvGetSize(hsvFrame).height); y++) 
	{
		for(x = 0; x < (cvGetSize(hsvFrame).width); x++) 
		{
			val_H = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3  ];
			val_S = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+1];
			val_V = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+2];
			
			if(!(	(( val_H >=40 && val_H <=95) ) &&
					( val_S<0 || val_S>100) ) )
			// vert : de 40 à 95
			// peu grisé : saturation <= 0
			// image sombre ou claire : valeur <= 200
			{
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+1] = 0;
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+2] = 0;
			}
			else
			{
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+1] = 0;
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+2] = 255;
			}
		}
	} //fin des boucles
	return 0;
}
int traiterbleu (IplImage* hsvFrame)
// traiterbleu transforme l'image entrante en bleu
//					fonctionnement : les pixels non bleu ou trop sombre ou trop gris sont transformés en noir
// <hsvFrame> : image qui doit etre allouée, en codage HSV
{
	//Pour chaque pixel…
	int x, y;
	int val_H, val_S, val_V;
	for(y = 0; y < (cvGetSize(hsvFrame).height); y++) 
	{
		for(x = 0; x < (cvGetSize(hsvFrame).width); x++) 
		{
			val_H = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3  ];
			val_S = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+1];
			val_V = (hsvFrame->imageData + hsvFrame->widthStep*y)[x*3+2];
			
			if(!(	(( val_H >=-150 && val_H <=-120) || (val_H >=90 && val_H <=180) ) &&
					( val_S<=0 ) &&
					( val_V<=200) ) )
			// bleu : de -150 à -120 et de 90 à 180
			// peu grisé : saturation < 0 (pas le blanc)
			// image sombre ou claire : valeur <= 200
			{
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+1] = 0;
				(hsvFrame->imageData + hsvFrame->widthStep*y)[ x*3+2] = 0;
			}
		}
	} //fin des boucles
	return 0;
}


// ---------------------------------------------------------------------------
// Fonctions de création et destruction de fenetres
void creerfenetreslimites(int option)
// creerfenetreslimites crée et positionne les fenetres de détection du vert
// <option> : 	1 = grande taille
// 				2 = petite taille
{
    cvNamedWindow( wdnbrg			, option);
    cvNamedWindow( wdnresultvert	, option);
    cvNamedWindow( wdnvert			, option);
    cvNamedWindow( wdnvertdilate	, option);
    cvNamedWindow( wdnvertcvcanny	, option);
    cvNamedWindow( wdnvertlimites	, option);
	cvNamedWindow(wdndistorsion		, option);
    
	cvMoveWindow(wdnbrg			,   0,    0);
	cvMoveWindow(wdnresultvert	,   0,  250);
	cvMoveWindow(wdnvert		, 320,  250);
	cvMoveWindow(wdnvertdilate	,   0,  500);
	cvMoveWindow(wdnvertcvcanny	, 640,  250);
	cvMoveWindow(wdnvertlimites	, 920,  250);
	cvMoveWindow(wdndistorsion	, 920,  500);
}

void creerfenetresperspective(int option)
// creerfenetresperspective crée et positionne les fenetres de détection des limites du terrain
//							c'est la seule seule des fonctions réellement appelées, les autres servant aux tests
// <option> : 	1 = grande taille
// 				2 = petite taille
{
    cvNamedWindow( wdnbrg			, option);
    cvNamedWindow( wdnbords			, option);
    cvNamedWindow( wdnperspective	, option);
    
	cvMoveWindow(wdnbrg			,   0,    0);
	cvMoveWindow(wdnbords		,   0,  250);
	cvMoveWindow(wdnperspective	,   0,  500);
}

void creerfenetres(int option)
// creerfenetres crée et positionne les fenetres de détection d'objets, sert aux tests
// <option> : 	1 = grande taille
// 				2 = petite taille
{
    cvNamedWindow( wdnresultrouge	, option);
    cvNamedWindow( wdnmotionrouge	, option);
    cvNamedWindow( wdnresultbleu	, option);
    cvNamedWindow( wdnmotionbleu	, option);
    
	cvMoveWindow(wdnresultrouge	, 320,  250);
	cvMoveWindow(wdnmotionrouge	, 640,  250);
	cvMoveWindow(wdnresultbleu	, 320,  500);
	cvMoveWindow(wdnmotionbleu	, 640,  500);
}

void detruirefenetreslimites()
{
    cvDestroyWindow( wdnbrg			);
    cvDestroyWindow( wdnresultvert	);
    cvDestroyWindow( wdnvert		);
    cvDestroyWindow( wdnvertdilate	);
    cvDestroyWindow( wdnvertcvcanny	);
    cvDestroyWindow( wdnvertlimites	);
    cvDestroyWindow( wdndistorsion	);
}

void detruirefenetresperspective()
{
    cvDestroyWindow( wdnbrg			);
    cvDestroyWindow( wdnbords		);
    cvDestroyWindow( wdnperspective	);
}

void detruirefenetres()
{
    cvDestroyWindow( wdnresultrouge	);
    cvDestroyWindow( wdnmotionrouge	);
    cvDestroyWindow( wdnresultbleu	);
    cvDestroyWindow( wdnmotionbleu	);
}


// ---------------------------------------------------------------------------
// Fonctions utilisées pour les calculs "mathématiques"
float distancecarre(positionRectangle obj1, positionRectangle obj2)
// calcul le carre de la distance entre 2 points
{
	return pow(obj1.center.x - obj2.center.x, 2) + pow(obj1.center.x - obj2.center.x, 2);
}

int trouverunelimite(ligneThetaRho lignes[], int nbLignes, orientationObjet orientation, int lignedejautilisee)
// trouverunelimite trouve le numéro de la ligne le plus en haut, ou a gauche, ou a droite, ou en bas
// <lignes> :			tableau des lignes du terrain
// <nbLignes> :			nombre de lignes
// <orientation> :		type de ligne à détecter (HAUT, GAUCHE, BAS ou DROITE)
// <lignedejautilisee> :pour ne pas utiliser la meme ligne en HAUT et BAS ou GAUCHE et DROITE
{
	if (orientation == AUCUN || nbLignes<=0)
	{
		return -1;
	}
	int resultat = -1;
	int tmp;
	if (orientation == HAUT)
	{
		float rhomin = 10000;
		for (tmp = 0; tmp<nbLignes ; tmp++)
		{
			if ((cos(lignes[tmp].theta) < 0.15) && (lignes[tmp].rho < rhomin) && (tmp != lignedejautilisee))
			{
				rhomin = lignes[tmp].rho;
				resultat = tmp;
			}
		}
	}
	if (orientation == BAS)
	{
		float rhomax = 0;
		for (tmp = 0; tmp<nbLignes ; tmp++)
		{
			if ((cos(lignes[tmp].theta) < 0.15) && (lignes[tmp].rho > rhomax) && (tmp != lignedejautilisee))
			{
				rhomax = lignes[tmp].rho;
				resultat = tmp;
			}
		}
	}
	if (orientation == GAUCHE)
	{
		float rhomin = 10000;
		for (tmp = 0; tmp<nbLignes ; tmp++)
		{
			if ( (-0.15 < sin(lignes[tmp].theta)) && (sin(lignes[tmp].theta)< 0.15) && (lignes[tmp].rho < rhomin) && (tmp != lignedejautilisee))
			{
				rhomin = lignes[tmp].rho;
				resultat = tmp;
			}
		}
	}
	if (orientation == DROITE)
	{
		float rhomax = 0;
		for (tmp = 0; tmp<nbLignes ; tmp++)
		{
			if ( (-0.15 < sin(lignes[tmp].theta)) && (sin(lignes[tmp].theta)< 0.15) && (lignes[tmp].rho > rhomax) && (tmp != lignedejautilisee))
			{
				rhomax = lignes[tmp].rho;
				resultat = tmp;
			}
		}
	}
	
	return resultat;
}

void calculintersection(ligneThetaRho ligne1, ligneThetaRho ligne2, CvPoint* croisement)
// calculintersection utilise ligne1 et ligne2 pour déterminer les coordonnées de croisement, leur point d'intersection
{
	float rho1 = ligne1.rho;
	float rho2 = ligne2.rho;
	float theta1 = ligne1.theta;
	float theta2 = ligne2.theta;
	if (theta1 == 0)
	{
		croisement->x = rho1;
		croisement->y = cvRound((rho2 - (rho1 * cos(theta2))) / sin(theta2));
		return;
	}
	if (theta2 == 0)
	{
		croisement->x = rho2;
		croisement->y = cvRound((rho1 - (rho2 * cos(theta1))) / sin(theta1));
		return;
	}
	croisement->x = cvRound( (rho2 * sin(theta1) - rho1 * sin(theta2)) / sin(theta1 - theta2));
	croisement->y = cvRound( (rho2 - ((croisement->x) * cos(theta2))) / sin(theta2));
	return;
}

void tracerligne(IplImage* image, ligneThetaRho* ligne, CvScalar* color)
// tracerligne dessine sur <image> la <ligne> voulue dans la couleur <color>
{
    double a, b;
    double x0, y0;
    CvPoint pt1, pt2;
    a = cos(ligne->theta);
    b = sin(ligne->theta);
    x0 = a*ligne->rho;
    y0 = b*ligne->rho;
    pt1.x = cvRound(x0 + 1000*(-b));
    pt1.y = cvRound(y0 + 1000*(a));
    pt2.x = cvRound(x0 - 1000*(-b));
    pt2.y = cvRound(y0 - 1000*(a));
	cvLine( image, pt1, pt2, *color, 3, 8, 0 );
}

int trouvermatriceperspective(CvPoint2D32f pts_input[], CvPoint2D32f pts_output[], CvMat* mmat)
// trouvermatriceperspective déduit une matrice de perspective à partir des points de l'image de départ et celle suivant la transformation
// <pts_input> :	matrice des points de l'image avant le changement de perspective. Doit etre alloué et de taille 4 !
// <pts_output> :	matrice des points de l'image après le changement de perspective. Doit etre alloué et de taille 4 !
// <mmat> :			matrice de changement de perspective
// pts_input et pts_output doivent respecter le meme ordre interne
{
	CvPoint2D32f c1[4];
    CvPoint2D32f c2[4];
    int i;
    for (i=0; i<4; i++)
    {
    	c1[i] = pts_input[i];
    	c2[i] = pts_output[i];
    }
    
	mmat = cvGetPerspectiveTransform(c1, c2, mmat);
	
	return 0;
}

int arrangerimage(IplImage* image_input, IplImage* image_output, CvMat* mmat)
// arrangerimage transforme l'image en changeant la perspective
{
	cvWarpPerspective(image_input, image_output, mmat, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
	return 0;
}
